// 寒假考核代码头文件+PNP解算

#pragma once

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <algorithm>
#include <sstream>

extern float globalTwoVelocity;  //二维速度
extern float globalDistance;  //相机到装甲板中心的距离

cv::Mat ColorFind(cv::Mat srcImage,     //创建一个ColorFind函数，用于颜色分割，图像二值化以及颜色识别
                  int iLowH,int iHighH,
                  int iLowS,int iHighS,
                  int iLowV,int iHighV);

class ColorFind_RGB  //创建一个围绕红蓝颜色分割，图像二值化的类
{
public:
       cv::Mat ColorFind_Red(cv::Mat srcImage);
       cv::Mat ColorFind_Blue(cv::Mat srcImage);
};

class ColorBool_RGB  //创建一个判断红蓝的类
{
public:
       bool ColorFindRedBool(cv::Mat srcImage);
       bool ColorFindBlueBool(cv::Mat srcImage);
};

class LightDescriptor  //创建一个存放灯条信息的类
{
public:
    LightDescriptor() {};  //构造函数，用于初始化其成员变量
    LightDescriptor(const cv::RotatedRect& light) //创建灯条函数
    {
        width = light.size.width;
        length = light.size.height;
        center = light.center;
        angle = light.angle;
        area = light.size.area();
    };
    const LightDescriptor& operator =(const LightDescriptor& ld)
    {
        this->width = ld.width;
        this->length = ld.length;
        this->center = ld.center;
        this->angle = ld.angle;
        this->area = ld.area;
        return *this;
    };
    float width, length, angle, area;  //创建灯条的属性变量，包括宽度，长度，角度，面积
    cv::Point2f center;  //创建灯条中心坐标的变量
};

class ArmorVelocity  //创建一个存放用于测量装甲板速度的类
{
private:
    cv::Point2f previousVertex; // 上一帧的顶点位置
    cv::Point2f currentVelocity; // 当前速度（x分量速度，y分量速度）
    float TwoVelocity;   //二维速度
    double frameRate; //帧率
    bool isFirstFrame; //标记是否为第一帧
public:
    // 构造函数，传入帧率、上一帧顶点位置、当前速度、二维速度，并初始化
    ArmorVelocity(double rate) : previousVertex(0, 0), currentVelocity(0, 0), TwoVelocity(0) , frameRate(rate) {};
    //测量装甲板二维速度
    void measureVelocity(const cv::Point2f& currentVertex);
};

class ArmorInfo {      //创建一个存放装甲板位置、速度信息的类
public:
    cv::Point2f vertex;
    ArmorVelocity velocity;
    //构造函数，传入速度，并初始化
    ArmorInfo(double frameRate) : velocity(frameRate) {}
    //更新顶点信息以继续计算速度
    void updateVertices(const cv::Point2f& newVertex);
};

class DetectOperation  //创建一个存放识别操作的类
{
public:
    //识别红蓝
    void findRedOrBlue(cv::Mat & inputImage,cv::Mat & outputImage,std::string & color);
    //图像处理
    void imageProcess(cv::Mat & inputImage,cv::Mat & outputImage);
    //筛选轮廓
    void filterContours(std::vector<std::vector<cv::Point>>& contours,std::vector<LightDescriptor>& lightInfos);
    //装甲板可视化，读取装甲板位置信息
    void armorDetect(std::vector<LightDescriptor>& lightInfos,cv::Mat & inputImage,std::vector<ArmorInfo>& armors,double frameRate);
};

class PnPSolution  //创建一个用于PNP解算的类
{
private:
    std::vector<cv::Point3f> objectPoints;  //三维点
    cv::Mat cameraMatrix; //相机内参
    cv::Mat distCoeffs;  //畸变参数
    cv::Mat rvec; //旋转向量
    cv::Mat tvec; //平移向量
public:
    //利用PNP解算出相机到物体的距离
    double calculateDistance(const std::vector<cv::Point2f> &imagePoints);
};
